// dbus_parser.c
#include "dbus_parser.h"
#include <string.h>

void DBUS_Init(DBUS_Parser *parser)
{
    memset(parser, 0, sizeof(DBUS_Parser));
    parser->connected = false;
}

bool DBUS_Update(DBUS_Parser *parser, uint8_t *data, uint32_t len)
{
    if (len != sizeof(DBUS_Data))
    {
        return false;
    }

    // 检查帧头
    if (data[0] != 0x03 || data[1] != 0x3F)
    {
        return false;
    }

    memcpy(&parser->raw_data, data, sizeof(DBUS_Data));
    parser->connected = true;

    // 解析通道和开关
    DBUS_ParseChannels(parser);

    return true;
}

void DBUS_ParseChannels(DBUS_Parser *parser)
{
    // 数据范围检查
    if (parser->raw_data.ch0 < DBUS_CH_MIN || parser->raw_data.ch0 > DBUS_CH_MAX ||
        parser->raw_data.ch1 < DBUS_CH_MIN || parser->raw_data.ch1 > DBUS_CH_MAX ||
        parser->raw_data.ch2 < DBUS_CH_MIN || parser->raw_data.ch2 > DBUS_CH_MAX ||
        parser->raw_data.ch3 < DBUS_CH_MIN || parser->raw_data.ch3 > DBUS_CH_MAX)
    {
        parser->connected = false;
        return;
    }

    // 将原始通道值转换为 -660 ~ 660 的范围
    parser->right_x = parser->raw_data.ch0 - DBUS_CH_MID;
    parser->right_y = parser->raw_data.ch1 - DBUS_CH_MID;
    parser->left_y = parser->raw_data.ch2 - DBUS_CH_MID;
    parser->left_x = parser->raw_data.ch3 - DBUS_CH_MID;

    // 解析开关状态
    parser->left_switch = parser->raw_data.switches & 0x03;
    parser->right_switch = (parser->raw_data.switches >> 2) & 0x03;
}

// 模拟生成DBUS数据
void DBUS_SimulateData(uint8_t *buffer, int16_t lx, int16_t ly, int16_t rx, int16_t ry, uint8_t l_sw, uint8_t r_sw)
{
    DBUS_Data *data = (DBUS_Data *)buffer;

    // 设置帧头
    data->header[0] = 0x03;
    data->header[1] = 0x3F;

    // 限制输入范围
    lx = (lx < -660) ? -660 : (lx > 660 ? 660 : lx);
    ly = (ly < -660) ? -660 : (ly > 660 ? 660 : ly);
    rx = (rx < -660) ? -660 : (rx > 660 ? 660 : rx);
    ry = (ry < -660) ? -660 : (ry > 660 ? 660 : ry);

    // 设置通道值 (将-660~660转换回364~1684)
    data->ch0 = rx + DBUS_CH_MID; // 右摇杆X
    data->ch1 = ry + DBUS_CH_MID; // 右摇杆Y
    data->ch2 = ly + DBUS_CH_MID; // 左摇杆Y
    data->ch3 = lx + DBUS_CH_MID; // 左摇杆X

    // 其他通道设为中位（根据你的结构体定义）
    data->ch4 = DBUS_CH_MID; // 拨轮1
    data->ch5 = DBUS_CH_MID; // 拨轮2
    data->ch6 = 0;           // 保留通道

    // 设置开关状态
    data->switches = (r_sw << 2) | l_sw;
    data->mouse_buttons = 0;
}